#include "mmu.h"
#include "pt_regs.h"
#include "sched.h"

unsigned long total_forks;

task_union init_task_union __init_task_data = {INIT_TASK(init_task_union.task)};

task_struct *g_task[NR_TASK] = {&init_task_union.task,};

static int find_empty_task(void){
    int i;
    for (int i; i < NR_TASK; i++){
        if (!g_task[i])
            return i;
    }

    return -1;
}

/* get pt_regs*/

static struct pt_regs* task_pt_regs(task_struct *tsk)
{
    unsigned long p;

    p = (unsigned long)tsk + THREAD_SIZE - sizeof(struct pt_regs);

    return (struct pt_regs*)p;
}

static int copy_thread(unsigned long clone_flags, task_struct *p,
        unsigned long fn, unsigned long arg)
{
    struct  pt_regs *childregs;

    childregs = task_pt_regs(p);
    memset(childregs, 0, sizeof(struct pt_regs));
    memset(&p->cpu_context, 0, sizeof(cpu_context));

    if(clone_flags & PF_KTHREAD){
        childregs -> pstate = PSR_MODE_EL1h;
        p->cpu_context.x19 = fn;
        p->cpu_context.x20 = arg;
    }

    p->cpu_context.pc = (unsigned long) ret_from_fork;
    p->cpu_context.sp = (unsigned long) childregs;

    return 0;
    
}


int do_fork(unsigned long clone_flags, unsigned long fn, unsigned long arg){
    task_struct *p;
    int pid;

    p = (task_struct *)get_free_page();
    if (!p)
        goto error;
    pid = find_empty_task();
    if (pid < 0)
        goto error;

    printk("test1------------1");

    if(copy_thread(clone_flags, p, fn, arg))
        goto error;
    printk("test2------------1");

    p->state = TASK_RUNNING;
    p->pid = pid;
    p->counter = (current->counter + 1) >> 1;
    current->counter >>=1;
    p->need_resched = 0;
	p->preempt_count = 0;
    p->priority = 2;
    total_forks++;
    g_task[pid] = p;
    printk("test3------------1");
    SET_LINKS(p);
    printk("test4------------1");

    wake_up_process(p);
    printk("test5------------1");

    return pid;    

error:
    return -1;
}

task_struct* switch_to(task_struct* pre, task_struct *next){
    
    printk("DEBUG 1 %s \n",__func__);
    
    if(pre == next)
        return NULL;

    cpu_switch_to(pre, next);
}

